﻿using System;
using UnityEngine;

namespace HotPlayer.Physic2D
{
    public static class EPA
    {
        public static Vector2 ExpandPolytope(Polygon2D shape1, Polygon2D shape2, Simplex simplex, out Vector2 contactPoint)
        {
            int maxIterations = 100;
            const double tolerance = 0.0001;

            while (maxIterations-- > 0)
            {
                int closestEdgeIndex = -1;
                var closestEdgeDistance = float.MaxValue;
                Vector2 closestEdgeNormal = new Vector2();

                for (int i = 0; i < simplex.Count; i++)
                {
                    int j = (i + 1) % simplex.Count;
                    Vector2 a = simplex[i];
                    Vector2 b = simplex[j];

                    Vector2 edge = b - a;
                    var normal = new Vector2(edge.y, -edge.x).normalized;

                    var distance = Vector2.Dot(normal, a);
                    if (distance < closestEdgeDistance)
                    {
                        closestEdgeDistance = distance;
                        closestEdgeNormal = normal;
                        closestEdgeIndex = i;
                    }
                }

                Vector2 support = GJKUtils.Support(shape1, shape2, closestEdgeNormal);

                var projection = Vector2.Dot(closestEdgeNormal, support);
                if (projection - closestEdgeDistance < tolerance)
                {
                    //contactPoint = ComputeContactPoint(shape1, shape2, closestEdgeNormal);
                    contactPoint = ComputeContactPointOnEdge(simplex[closestEdgeIndex], simplex[(closestEdgeIndex + 1) % simplex.Count], shape1, shape2, closestEdgeNormal);
                    return closestEdgeNormal * closestEdgeDistance;
                }

                simplex.Insert(closestEdgeIndex + 1, support);
            }

            throw new Exception("EPA failed to converge.");
        }
        
        private static Vector2 ComputeContactPoint(Polygon2D shape1, Polygon2D shape2, Vector2 normal)
        {
            Vector2 pointOnShape1 = shape1.Support(normal);
            Vector2 pointOnShape2 = shape2.Support(-normal);

            return (pointOnShape1 + pointOnShape2) * 0.5f;
        }
        
        private static Vector2 ComputeContactPointOnEdge(Vector2 a, Vector2 b, Polygon2D shape1, Polygon2D shape2, Vector2 normal)
        {
            Vector2 ab = b - a;
            var t = -Vector2.Dot(a, ab) / Vector2.Dot(ab, ab);
            t = Math.Max(0, Math.Min(1, t));

            Vector2 closestPointOnSimplex = a + ab * t;

            Vector2 pointOnShape1 = shape1.Support(normal);
            Vector2 pointOnShape2 = shape2.Support(-normal);

            Vector2 contactPoint = pointOnShape1 + (pointOnShape2 - pointOnShape1) * t;

            return contactPoint;
        }
    }
}